#include <tsubamegaeshi-base/worldmodel.h>

#include <iostream>
#include <boost/lexical_cast.hpp>

static void
dummy_callback(const boost::match_results<std::string::const_iterator>& m)
{
  
  std::cout << m.str(0) << std::endl;

  for (int i = 1; i < m.size(); i++) {
    std::cout << m.str(i) << ", ";
  }
  std::cout << m.size() << " datas. " << std::endl << std::endl;

}

void
Worldmodel::parse(const std::string& message)
{
  step_++;

  Parser::parse(message);

  current_status().calc_position();

  /*
  std::cout << "(NAME):"
            << teamname_
            << "NUM(" << number_ << ") "
            << "L_F(" << side_   << ") "
            <<std::endl;
  */
  //current_status().dump();
  
}

void
Worldmodel::time_callback(const boost::match_results<std::string::const_iterator>& m, int i=0)
{
  double last = status_history(1).time_;
  current_status().time_ = boost::lexical_cast<double>(m.str(i + 1));
  if (current_status().time_ - last > 0.0201) {
    std::cerr << "message interval slow(>0.02): "<< (current_status().time_ - last) << std::endl;
  }
}

void
Worldmodel::gs_callback(const boost::match_results<std::string::const_iterator>& m, int i=0)
{
  if (i == 0) {
    switch  (m.size()) {
    case 3:
      current_status().gs_time_ = boost::lexical_cast<double>(m.str(i + 1));
      current_status().gs_pm_ = m.str(i + 2);
      break;
    case 5:
      number_ = boost::lexical_cast<int>(m.str(i + 1));
      if (m.str(i + 2) == "left") {
        side_ = S_LEFT;
      } else if (m.str(i + 2) == "right") {
        side_ = S_RIGHT;
      }
      current_status().gs_time_ = boost::lexical_cast<double>(m.str(i + 3));
      current_status().gs_pm_ = m.str(i + 4);
      break;
    };
  } else if (i == 1) {
      current_status().gs_time_ = boost::lexical_cast<double>(m.str(i + 1));
      current_status().gs_pm_ = m.str(i + 2);
  } else if (i == 3) {
      number_ = boost::lexical_cast<int>(m.str(i + 1));
      if (m.str(i + 2) == "left") {
        side_ = S_LEFT;
      } else if (m.str(i + 2) == "right") {
        side_ = S_RIGHT;
      }
      current_status().gs_time_ = boost::lexical_cast<double>(m.str(i + 3));
      current_status().gs_pm_ = m.str(i + 4);
  }
}

void
Worldmodel::gyr_callback(const boost::match_results<std::string::const_iterator>& m, int i=0)
{
  current_status().gyr_[m.str(i + 1)]  = TVector3(boost::lexical_cast<double>(m.str(i + 2)),
                                         boost::lexical_cast<double>(m.str(i + 3)),
                                         boost::lexical_cast<double>(m.str(i + 4)));
}

void
Worldmodel::flag_callback(const boost::match_results<std::string::const_iterator>& m, int i=0)
{
  current_status().flag_[m.str(i + 1)] = TVector3(boost::lexical_cast<double>(m.str(i + 2)),
                                              boost::lexical_cast<double>(m.str(i + 3)),
                                              boost::lexical_cast<double>(m.str(i + 4)));
}

void
Worldmodel::ball_callback(const boost::match_results<std::string::const_iterator>& m, int i=0)
{
  current_status().ball_ = TVector3(boost::lexical_cast<double>(m.str(i + 1)),
                                    boost::lexical_cast<double>(m.str(i + 2)),
                                    boost::lexical_cast<double>(m.str(i + 3)));
}

void
Worldmodel::uj_callback(const boost::match_results<std::string::const_iterator>& m, int i=0)
{
  current_status().uj_[m.str(i + 1)] = std::pair< double, double>(boost::lexical_cast<double>(m.str(i + 2)),
                                                              boost::lexical_cast<double>(m.str(i + 3)));
}

void
Worldmodel::hj_callback(const boost::match_results<std::string::const_iterator>& m, int i=0)
{
  current_status().hj_[m.str(i + 1)] = boost::lexical_cast<double>(m.str(i + 2));
}

void
Worldmodel::frp_callback(const boost::match_results<std::string::const_iterator>& m, int i=0)
{
  current_status().frp_[m.str(i + 1)]  = TFrpValue(boost::lexical_cast<double>(m.str(i + 2)),
                                               boost::lexical_cast<double>(m.str(i + 3)),
                                               boost::lexical_cast<double>(m.str(i + 4)),
                                               boost::lexical_cast<double>(m.str(i + 5)),
                                               boost::lexical_cast<double>(m.str(i + 6)),
                                               boost::lexical_cast<double>(m.str(i + 7)));

}

void
Worldmodel::mypos_callback(const boost::match_results<std::string::const_iterator>& m, int i=0)
{
  current_status().position_mypos_ = TVector3(boost::lexical_cast<double>(m.str(i + 1)),
                                              boost::lexical_cast<double>(m.str(i + 2)),
                                              boost::lexical_cast<double>(m.str(i + 3)));
}


